# from util.LabelPoints2ImagebyMayaParam import *
import os
import json
import numpy as np
import math

import pandas as pd
import matplotlib.pyplot as plt
from iou3d_nms_utils import *
import time
from sklearn.metrics import confusion_matrix # 导入计算混淆矩阵的包
import seaborn as sns
from markdown import markdown
import pdfkit

# // Obstacle type
ObstacleType={
    0:'Invalid',
    'VEHICLE':'Vehicle',
    2:'Pedestrian',
    3:'Rider',
    4:'Traffic_cone',
    5:'Animal',
    6:'Road_debris',
    7:'Fence',
    'Vehicle':{
        0:'unknown',
        1:'bus',
        2:'car',
        3:'truck',
        4:'special',
        5:'tiny',
        6:'van'
    },
    'Rider':{
        0: 'unknown',
        1: 'cyclist',
        2: 'motorcyclist',
        3: 'tricyclist'
    },
    'Pedestrian':{
        0:'unknown',
        1:'adult',
        2:'child'
    }
}

data_path='/home/king/workplace/data/data27_2021_04_03/'
image_path='/home/king/workplace/data/data27_2021_04_03/pr_obfu_label_json/'
radar_file='obfu_data27.csv'
radar_data_name=data_path+radar_file
lidar2car = np.array([6.09969, -0.060999])
delta_phi = 1

radar_datas=pd.read_csv(radar_data_name)

radar_datas['datetime']=pd.to_datetime((radar_datas['timestamp']/1e6),unit='s')
timestamp_temp='0'
timestamp='0'
count_s=0
count_f=0
# 计算box中点
radar_datas['center_rel.x']=(radar_datas['bbox_top_left_x']+radar_datas['bbox_top_right_x']+radar_datas['bbox_bottom_right_x']+radar_datas['bbox_bottom_left_x'])/4
radar_datas['center_rel.y']=(radar_datas['bbox_top_left_y']+radar_datas['bbox_top_right_y']+radar_datas['bbox_bottom_right_y']+radar_datas['bbox_bottom_left_y'])/4

import datetime

dt_ms = datetime.datetime.now().strftime('%Y-%m-%d_%H-%M-%S-%f')[:3]

def get_type(type_data):
    name=[]
    for type in type_data:
        if type[0]!='VEHICLE':
            name.append('other')
        else:
            name.append(ObstacleType[ObstacleType[type[0]]][type[1]])
    return name

for Timestamp_index in radar_datas['timestamp'].unique():
    datas_temp=radar_datas[radar_datas['timestamp']==Timestamp_index]
    time_local = time.localtime(Timestamp_index / 1e6)
    timestamp = time.strftime("%Y-%m-%d_%H-%M-%S-", time_local)
    if timestamp_temp != timestamp:
        count_s = 0
    count_f += 1
    count_s += 1
    timestamp_temp = timestamp
    box = datas_temp.loc[:, ('center_rel.x','center_rel.y')].to_numpy().T.tolist() + (np.ones([datas_temp.__len__(),1])*(-2)).T.tolist()+ datas_temp.loc[:, ('length', 'width', 'height')].to_numpy().T.tolist()+ [datas_temp.loc[:,('heading_rel')].to_numpy().T.tolist()]
    box_temp = np.array(box).T
    # box_temp[:, 0:2] = box_temp[:,0:2] - lidar2car
    box_temp[:,0:2]=np.array([[np.cos(np.pi*(delta_phi/180)),np.sin(-np.pi*(delta_phi/180))],[np.sin(np.pi*(delta_phi/180)),np.cos(np.pi*(delta_phi/180))]]).T.dot(box_temp[:,0:2].T).T- lidar2car
    boxs = box_temp.tolist()
    data_temp = {
        'point_cloud': {
            'frame_id': count_f,
            'next_frame': count_f + 1,
            'previous_frame': count_f - 1,
            'point_file': -1,
            'offset_z': 1},
        'image_file': timestamp + str(count_s) + '.jpg',
        'radar_file': timestamp + str(count_s) + '.bin',
        'muti_sensor_cfg': 0,
        'annos': {
            'name': get_type(datas_temp.loc[:,('obstacle_type', 'vehicle_subtype')].to_numpy()),  # * str < ----- Check:-1 | shape: 1
            'gt_boxes_3d':boxs,
            'num_lidar_pts': -1,  # < ----- Check:-1 | shape: 1x
            'track_id': datas_temp['id'].tolist(),
            # < ----- Check:-1 | shape: 2
            'velocity': datas_temp.loc[:, ('velocity_rel.x', 'velocity_rel.y', 'velocity_rel_abs','velocity_rel_sd.x', 'velocity_rel_sd.y')].to_numpy().tolist(),  # float / -1 < ----- Check: -1 | shape: 2
            'acceleration': datas_temp.loc[:, ('acceleration_rel.x','acceleration_rel.y', 'acceleration_rel_abs', 'acceleration_rel_sd.x','acceleration_rel_sd.y')].to_numpy().tolist(),  # float / -1 < ----- Check: -1 | shape: 2
            'confidence': datas_temp.loc[:, ('confidence')].to_numpy().T.tolist(),
            'ego_velocity':datas_temp.loc[:, ('ego_velocity')].to_numpy().T.tolist(),
            'ego_acceleration':datas_temp.loc[:, ('ego_acceleration')].to_numpy().T.tolist(),
            'others': 0
        }
    }
    if not os.path.exists(image_path):
        os.mkdir(image_path)
    # print(data_temp)
    with open(image_path + timestamp + str(count_s) + '.json', 'w') as f:
        json.dump(data_temp, f)
